<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN" "http://www.w3.org/TR/html4/loose.dtd">
<html lang="en">
<head>
<title>Source code</title>
<link rel="stylesheet" type="text/css" href="../../../../stylesheet.css" title="Style">
</head>
<body>
<div class="sourceContainer">
<pre><span class="sourceLineNo">001</span>package imagingbook.calibration.zhang;<a name="line.1"></a>
<span class="sourceLineNo">002</span><a name="line.2"></a>
<span class="sourceLineNo">003</span>import java.awt.geom.Point2D;<a name="line.3"></a>
<span class="sourceLineNo">004</span>import java.util.ArrayList;<a name="line.4"></a>
<span class="sourceLineNo">005</span>import java.util.List;<a name="line.5"></a>
<span class="sourceLineNo">006</span>import java.util.Random;<a name="line.6"></a>
<span class="sourceLineNo">007</span><a name="line.7"></a>
<span class="sourceLineNo">008</span>import org.apache.commons.math3.analysis.MultivariateMatrixFunction;<a name="line.8"></a>
<span class="sourceLineNo">009</span>import org.apache.commons.math3.analysis.MultivariateVectorFunction;<a name="line.9"></a>
<span class="sourceLineNo">010</span>import org.apache.commons.math3.fitting.leastsquares.LeastSquaresFactory;<a name="line.10"></a>
<span class="sourceLineNo">011</span>import org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer;<a name="line.11"></a>
<span class="sourceLineNo">012</span>import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer.Optimum;<a name="line.12"></a>
<span class="sourceLineNo">013</span>import org.apache.commons.math3.fitting.leastsquares.LeastSquaresProblem;<a name="line.13"></a>
<span class="sourceLineNo">014</span>import org.apache.commons.math3.linear.MatrixUtils;<a name="line.14"></a>
<span class="sourceLineNo">015</span>import org.apache.commons.math3.linear.RealMatrix;<a name="line.15"></a>
<span class="sourceLineNo">016</span>import org.apache.commons.math3.linear.RealVector;<a name="line.16"></a>
<span class="sourceLineNo">017</span><a name="line.17"></a>
<span class="sourceLineNo">018</span>import imagingbook.calibration.zhang.util.MathUtil;<a name="line.18"></a>
<span class="sourceLineNo">019</span>import imagingbook.lib.math.Matrix;<a name="line.19"></a>
<span class="sourceLineNo">020</span><a name="line.20"></a>
<span class="sourceLineNo">021</span>/**<a name="line.21"></a>
<span class="sourceLineNo">022</span> * This class defines methods for estimating the homography (projective)<a name="line.22"></a>
<span class="sourceLineNo">023</span> * transformation between pairs of 2D point sets.<a name="line.23"></a>
<span class="sourceLineNo">024</span> * @author WB<a name="line.24"></a>
<span class="sourceLineNo">025</span> */<a name="line.25"></a>
<span class="sourceLineNo">026</span>public class HomographyEstimator {<a name="line.26"></a>
<span class="sourceLineNo">027</span>        <a name="line.27"></a>
<span class="sourceLineNo">028</span>        private static int maxLmEvaluations = 1000;<a name="line.28"></a>
<span class="sourceLineNo">029</span>        private static int maxLmIterations = 1000;<a name="line.29"></a>
<span class="sourceLineNo">030</span>        <a name="line.30"></a>
<span class="sourceLineNo">031</span>        private boolean normalizePointCoordinates = true;<a name="line.31"></a>
<span class="sourceLineNo">032</span>        <a name="line.32"></a>
<span class="sourceLineNo">033</span>        public HomographyEstimator() {<a name="line.33"></a>
<span class="sourceLineNo">034</span>        }<a name="line.34"></a>
<span class="sourceLineNo">035</span>        <a name="line.35"></a>
<span class="sourceLineNo">036</span>        public HomographyEstimator(boolean normalizePointCoordinates) {<a name="line.36"></a>
<span class="sourceLineNo">037</span>                this.normalizePointCoordinates = normalizePointCoordinates;<a name="line.37"></a>
<span class="sourceLineNo">038</span>        }<a name="line.38"></a>
<span class="sourceLineNo">039</span>        <a name="line.39"></a>
<span class="sourceLineNo">040</span>        /**<a name="line.40"></a>
<span class="sourceLineNo">041</span>         * Estimates the homographies between a fixed set of 2D model points and<a name="line.41"></a>
<span class="sourceLineNo">042</span>         * multiple observations (image point sets).<a name="line.42"></a>
<span class="sourceLineNo">043</span>         * The correspondence between the points is assumed to be known.<a name="line.43"></a>
<span class="sourceLineNo">044</span>         * @param modelPts a sequence of 2D points on the model (calibration target)<a name="line.44"></a>
<span class="sourceLineNo">045</span>         * @param obsPoints a sequence 2D image point sets (one set per view).<a name="line.45"></a>
<span class="sourceLineNo">046</span>         * @return the sequence of estimated homographies (3 x 3 matrices), one for each view<a name="line.46"></a>
<span class="sourceLineNo">047</span>         */<a name="line.47"></a>
<span class="sourceLineNo">048</span>        public RealMatrix[] estimateHomographies(Point2D[] modelPts, Point2D[][] obsPoints) {<a name="line.48"></a>
<span class="sourceLineNo">049</span>                final int M = obsPoints.length;<a name="line.49"></a>
<span class="sourceLineNo">050</span>                RealMatrix[] homographies = new RealMatrix[M];<a name="line.50"></a>
<span class="sourceLineNo">051</span>                for (int i = 0; i &lt; M; i++) {<a name="line.51"></a>
<span class="sourceLineNo">052</span>                        RealMatrix Hinit = estimateHomography(modelPts, obsPoints[i]);<a name="line.52"></a>
<span class="sourceLineNo">053</span>                        RealMatrix H = refineHomography(Hinit, modelPts, obsPoints[i]);<a name="line.53"></a>
<span class="sourceLineNo">054</span>                        homographies[i] = H;<a name="line.54"></a>
<span class="sourceLineNo">055</span>                }<a name="line.55"></a>
<span class="sourceLineNo">056</span>                return homographies;<a name="line.56"></a>
<span class="sourceLineNo">057</span>        }<a name="line.57"></a>
<span class="sourceLineNo">058</span>        <a name="line.58"></a>
<span class="sourceLineNo">059</span>        /**<a name="line.59"></a>
<span class="sourceLineNo">060</span>         * Estimates the homography (projective) transformation from two given 2D point sets.<a name="line.60"></a>
<span class="sourceLineNo">061</span>         * The correspondence between the points is assumed to be known.<a name="line.61"></a>
<span class="sourceLineNo">062</span>         * @param ptsA the 1st sequence of 2D points <a name="line.62"></a>
<span class="sourceLineNo">063</span>         * @param ptsB the 2nd sequence of 2D points<a name="line.63"></a>
<span class="sourceLineNo">064</span>         * @return the estimated homography (3 x 3 matrix)<a name="line.64"></a>
<span class="sourceLineNo">065</span>         */<a name="line.65"></a>
<span class="sourceLineNo">066</span>        public RealMatrix estimateHomography(Point2D[] ptsA, Point2D[] ptsB) {<a name="line.66"></a>
<span class="sourceLineNo">067</span>                int n = ptsA.length;<a name="line.67"></a>
<span class="sourceLineNo">068</span>                <a name="line.68"></a>
<span class="sourceLineNo">069</span>                RealMatrix Na = (normalizePointCoordinates) ? getNormalisationMatrix(ptsA) : MatrixUtils.createRealIdentityMatrix(3);<a name="line.69"></a>
<span class="sourceLineNo">070</span>                RealMatrix Nb = (normalizePointCoordinates) ? getNormalisationMatrix(ptsB) : MatrixUtils.createRealIdentityMatrix(3);   <a name="line.70"></a>
<span class="sourceLineNo">071</span>                RealMatrix M = MatrixUtils.createRealMatrix(n * 2, 9);<a name="line.71"></a>
<span class="sourceLineNo">072</span><a name="line.72"></a>
<span class="sourceLineNo">073</span>                for (int j = 0, r = 0; j &lt; ptsA.length; j++) {<a name="line.73"></a>
<span class="sourceLineNo">074</span>                        final double[] pA = transform(MathUtil.toArray(ptsA[j]), Na);<a name="line.74"></a>
<span class="sourceLineNo">075</span>                        final double[] pB = transform(MathUtil.toArray(ptsB[j]), Nb);<a name="line.75"></a>
<span class="sourceLineNo">076</span>                        final double xA = pA[0];<a name="line.76"></a>
<span class="sourceLineNo">077</span>                        final double yA = pA[1];<a name="line.77"></a>
<span class="sourceLineNo">078</span>                        final double xB = pB[0];<a name="line.78"></a>
<span class="sourceLineNo">079</span>                        final double yB = pB[1];                        <a name="line.79"></a>
<span class="sourceLineNo">080</span>                        M.setRow(r + 0, new double[] {xA, yA, 1, 0, 0, 0, -(xA * xB), -(yA * xB), -(xB)});<a name="line.80"></a>
<span class="sourceLineNo">081</span>                        M.setRow(r + 1, new double[] {0, 0, 0, xA, yA, 1, -(xA * yB), -(yA * yB), -(yB)});<a name="line.81"></a>
<span class="sourceLineNo">082</span>                        r = r + 2;<a name="line.82"></a>
<span class="sourceLineNo">083</span>                }<a name="line.83"></a>
<span class="sourceLineNo">084</span><a name="line.84"></a>
<span class="sourceLineNo">085</span>                // find h, such that M . h = 0:<a name="line.85"></a>
<span class="sourceLineNo">086</span>                double[] h = MathUtil.solveHomogeneousSystem(M).toArray();<a name="line.86"></a>
<span class="sourceLineNo">087</span>                <a name="line.87"></a>
<span class="sourceLineNo">088</span>                // assemble homography matrix H from h:<a name="line.88"></a>
<span class="sourceLineNo">089</span>                RealMatrix H = MatrixUtils.createRealMatrix(new double[][] <a name="line.89"></a>
<span class="sourceLineNo">090</span>                                {{h[0], h[1], h[2]},<a name="line.90"></a>
<span class="sourceLineNo">091</span>                                 {h[3], h[4], h[5]},<a name="line.91"></a>
<span class="sourceLineNo">092</span>                                 {h[6], h[7], h[8]}} );<a name="line.92"></a>
<span class="sourceLineNo">093</span><a name="line.93"></a>
<span class="sourceLineNo">094</span>                // de-normalize the homography<a name="line.94"></a>
<span class="sourceLineNo">095</span>                H = MatrixUtils.inverse(Nb).multiply(H).multiply(Na);<a name="line.95"></a>
<span class="sourceLineNo">096</span>                <a name="line.96"></a>
<span class="sourceLineNo">097</span>                // rescale M such that H[2][2] = 1 (unless H[2][2] close to 0)<a name="line.97"></a>
<span class="sourceLineNo">098</span>                if (Math.abs(H.getEntry(2, 2)) &gt; 10e-8) {<a name="line.98"></a>
<span class="sourceLineNo">099</span>                        H = H.scalarMultiply(1.0 / H.getEntry(2, 2));<a name="line.99"></a>
<span class="sourceLineNo">100</span>                }<a name="line.100"></a>
<span class="sourceLineNo">101</span>                return H;<a name="line.101"></a>
<span class="sourceLineNo">102</span>        }<a name="line.102"></a>
<span class="sourceLineNo">103</span>        <a name="line.103"></a>
<span class="sourceLineNo">104</span><a name="line.104"></a>
<span class="sourceLineNo">105</span>        /**<a name="line.105"></a>
<span class="sourceLineNo">106</span>         * Refines the initial homography by non-linear (Levenberg-Marquart) optimization.<a name="line.106"></a>
<span class="sourceLineNo">107</span>         * @param Hinit the initial (estimated) homography matrix<a name="line.107"></a>
<span class="sourceLineNo">108</span>         * @param pntsA the 1st sequence of 2D points <a name="line.108"></a>
<span class="sourceLineNo">109</span>         * @param pntsB the 2nd sequence of 2D points<a name="line.109"></a>
<span class="sourceLineNo">110</span>         * @return the refined homography matrix<a name="line.110"></a>
<span class="sourceLineNo">111</span>         */<a name="line.111"></a>
<span class="sourceLineNo">112</span>        public RealMatrix refineHomography(RealMatrix Hinit, Point2D[] pntsA, Point2D[] pntsB) {<a name="line.112"></a>
<span class="sourceLineNo">113</span>                final int M = pntsA.length;             <a name="line.113"></a>
<span class="sourceLineNo">114</span>                double[] observed = new double[2 * M];<a name="line.114"></a>
<span class="sourceLineNo">115</span>                for (int i = 0; i &lt; M; i++) {<a name="line.115"></a>
<span class="sourceLineNo">116</span>                        observed[i * 2 + 0] = pntsB[i].getX();<a name="line.116"></a>
<span class="sourceLineNo">117</span>                        observed[i * 2 + 1] = pntsB[i].getY();<a name="line.117"></a>
<span class="sourceLineNo">118</span>                }                       <a name="line.118"></a>
<span class="sourceLineNo">119</span>                MultivariateVectorFunction value = getValueFunction(pntsA);<a name="line.119"></a>
<span class="sourceLineNo">120</span>                MultivariateMatrixFunction jacobian = getJacobianFunction(pntsA);<a name="line.120"></a>
<span class="sourceLineNo">121</span>                <a name="line.121"></a>
<span class="sourceLineNo">122</span>                LeastSquaresProblem problem = LeastSquaresFactory.create(<a name="line.122"></a>
<span class="sourceLineNo">123</span>                                LeastSquaresFactory.model(value, jacobian),<a name="line.123"></a>
<span class="sourceLineNo">124</span>                                MatrixUtils.createRealVector(observed), <a name="line.124"></a>
<span class="sourceLineNo">125</span>                                MathUtil.getRowPackedVector(Hinit), <a name="line.125"></a>
<span class="sourceLineNo">126</span>                                null,  // ConvergenceChecker<a name="line.126"></a>
<span class="sourceLineNo">127</span>                                maxLmEvaluations, <a name="line.127"></a>
<span class="sourceLineNo">128</span>                                maxLmIterations);<a name="line.128"></a>
<span class="sourceLineNo">129</span>                <a name="line.129"></a>
<span class="sourceLineNo">130</span>                LevenbergMarquardtOptimizer lm = new LevenbergMarquardtOptimizer();<a name="line.130"></a>
<span class="sourceLineNo">131</span>                Optimum result = lm.optimize(problem);<a name="line.131"></a>
<span class="sourceLineNo">132</span>                <a name="line.132"></a>
<span class="sourceLineNo">133</span>                RealVector optimum = result.getPoint();<a name="line.133"></a>
<span class="sourceLineNo">134</span>                RealMatrix Hopt = MathUtil.fromRowPackedVector(optimum, 3, 3);<a name="line.134"></a>
<span class="sourceLineNo">135</span>//              System.out.println("LM optimizer iterations " + result.getIterations());<a name="line.135"></a>
<span class="sourceLineNo">136</span>                return Hopt.scalarMultiply(1.0 / Hopt.getEntry(2, 2));<a name="line.136"></a>
<span class="sourceLineNo">137</span>        }<a name="line.137"></a>
<span class="sourceLineNo">138</span>        <a name="line.138"></a>
<span class="sourceLineNo">139</span>        <a name="line.139"></a>
<span class="sourceLineNo">140</span>        private MultivariateVectorFunction getValueFunction(Point2D[] X) {<a name="line.140"></a>
<span class="sourceLineNo">141</span>                //System.out.println("MultivariateVectorFunction getValueFunction");<a name="line.141"></a>
<span class="sourceLineNo">142</span>                return new MultivariateVectorFunction() {<a name="line.142"></a>
<span class="sourceLineNo">143</span>                        public double[] value(double[] h) {<a name="line.143"></a>
<span class="sourceLineNo">144</span>                                final double[] Y = new double[X.length * 2];<a name="line.144"></a>
<span class="sourceLineNo">145</span>                                for (int j = 0; j &lt; X.length; j++) {<a name="line.145"></a>
<span class="sourceLineNo">146</span>                                        final double x = X[j].getX();<a name="line.146"></a>
<span class="sourceLineNo">147</span>                                        final double y = X[j].getY();<a name="line.147"></a>
<span class="sourceLineNo">148</span>                                        final double w = h[6] * x + h[7] * y + h[8];<a name="line.148"></a>
<span class="sourceLineNo">149</span>                                        Y[j * 2 + 0] = (h[0] * x + h[1] * y + h[2]) / w;<a name="line.149"></a>
<span class="sourceLineNo">150</span>                                        Y[j * 2 + 1] = (h[3] * x + h[4] * y + h[5]) / w;<a name="line.150"></a>
<span class="sourceLineNo">151</span>                                }<a name="line.151"></a>
<span class="sourceLineNo">152</span>                                return Y;<a name="line.152"></a>
<span class="sourceLineNo">153</span>                        }<a name="line.153"></a>
<span class="sourceLineNo">154</span>                };<a name="line.154"></a>
<span class="sourceLineNo">155</span>        }<a name="line.155"></a>
<span class="sourceLineNo">156</span>        <a name="line.156"></a>
<span class="sourceLineNo">157</span>        protected MultivariateMatrixFunction getJacobianFunction(Point2D[] X) {<a name="line.157"></a>
<span class="sourceLineNo">158</span>                return new MultivariateMatrixFunction() {<a name="line.158"></a>
<span class="sourceLineNo">159</span>                        public double[][] value(double[] h) {<a name="line.159"></a>
<span class="sourceLineNo">160</span>                                final double[][] J = new double[2 * X.length][];<a name="line.160"></a>
<span class="sourceLineNo">161</span>                                for (int i = 0; i &lt; X.length; i++) {<a name="line.161"></a>
<span class="sourceLineNo">162</span>                                        final double x = X[i].getX();<a name="line.162"></a>
<span class="sourceLineNo">163</span>                                        final double y = X[i].getY();<a name="line.163"></a>
<span class="sourceLineNo">164</span>                                        <a name="line.164"></a>
<span class="sourceLineNo">165</span>                                        final double w  = h[6] * x + h[7] * y + h[8];<a name="line.165"></a>
<span class="sourceLineNo">166</span>                                        final double w2 = w * w;<a name="line.166"></a>
<span class="sourceLineNo">167</span>                                        <a name="line.167"></a>
<span class="sourceLineNo">168</span>                                        final double sx = h[0] * x + h[1] * y + h[2];           <a name="line.168"></a>
<span class="sourceLineNo">169</span>                                        J[2 * i + 0] = new double[] {x/w, y/w, 1/w, 0, 0, 0, -sx*x/w2, -sx*y/w2, -sx/w2};<a name="line.169"></a>
<span class="sourceLineNo">170</span>                                        <a name="line.170"></a>
<span class="sourceLineNo">171</span>                                        final double sy = h[3] * x + h[4] * y + h[5];<a name="line.171"></a>
<span class="sourceLineNo">172</span>                                        J[2 * i + 1] = new double[] {0, 0, 0, x/w, y/w, 1/w, -sy*x/w2, -sy*y/w2, -sy/w2};<a name="line.172"></a>
<span class="sourceLineNo">173</span>                                }<a name="line.173"></a>
<span class="sourceLineNo">174</span>                                return J;<a name="line.174"></a>
<span class="sourceLineNo">175</span>                        }<a name="line.175"></a>
<span class="sourceLineNo">176</span>                };<a name="line.176"></a>
<span class="sourceLineNo">177</span>        }<a name="line.177"></a>
<span class="sourceLineNo">178</span><a name="line.178"></a>
<span class="sourceLineNo">179</span>/*<a name="line.179"></a>
<span class="sourceLineNo">180</span>        protected MultivariateMatrixFunction getJacobianFunction(final Point2D[] X) {<a name="line.180"></a>
<span class="sourceLineNo">181</span>                return new MultivariateMatrixFunction() {<a name="line.181"></a>
<span class="sourceLineNo">182</span>                        @Override<a name="line.182"></a>
<span class="sourceLineNo">183</span>                        public double[][] value(double[] h) {<a name="line.183"></a>
<span class="sourceLineNo">184</span>                                final double[][] J = new double[2 * X.length][9];<a name="line.184"></a>
<span class="sourceLineNo">185</span>                                for (int i = 0; i &lt; X.length; i++) {<a name="line.185"></a>
<span class="sourceLineNo">186</span>                                        final double x = X[i].getX();<a name="line.186"></a>
<span class="sourceLineNo">187</span>                                        final double y = X[i].getY();<a name="line.187"></a>
<span class="sourceLineNo">188</span>                                        <a name="line.188"></a>
<span class="sourceLineNo">189</span>                                        final double w  = h[6] * x + h[7] * y + h[8];<a name="line.189"></a>
<span class="sourceLineNo">190</span>                                        final double w2 = w * w;<a name="line.190"></a>
<span class="sourceLineNo">191</span>                                        <a name="line.191"></a>
<span class="sourceLineNo">192</span>                                        final double sx = h[0] * x + h[1] * y + h[2];           <a name="line.192"></a>
<span class="sourceLineNo">193</span>                                        J[2 * i + 0][0] = x / w;<a name="line.193"></a>
<span class="sourceLineNo">194</span>                                        J[2 * i + 0][1] = y / w;<a name="line.194"></a>
<span class="sourceLineNo">195</span>                                        J[2 * i + 0][2] = 1.0 / w;<a name="line.195"></a>
<span class="sourceLineNo">196</span>                                        J[2 * i + 0][3] = 0;<a name="line.196"></a>
<span class="sourceLineNo">197</span>                                        J[2 * i + 0][4] = 0;<a name="line.197"></a>
<span class="sourceLineNo">198</span>                                        J[2 * i + 0][5] = 0;<a name="line.198"></a>
<span class="sourceLineNo">199</span>                                        J[2 * i + 0][6] = -sx * x / w2;<a name="line.199"></a>
<span class="sourceLineNo">200</span>                                        J[2 * i + 0][7] = -sx * y / w2;<a name="line.200"></a>
<span class="sourceLineNo">201</span>                                        J[2 * i + 0][8] = -sx / w2;<a name="line.201"></a>
<span class="sourceLineNo">202</span>                                        <a name="line.202"></a>
<span class="sourceLineNo">203</span>                                        final double sy = h[3] * x + h[4] * y + h[5];<a name="line.203"></a>
<span class="sourceLineNo">204</span>                                        J[2 * i + 1][0] = 0;<a name="line.204"></a>
<span class="sourceLineNo">205</span>                                        J[2 * i + 1][1] = 0;<a name="line.205"></a>
<span class="sourceLineNo">206</span>                                        J[2 * i + 1][2] = 0;<a name="line.206"></a>
<span class="sourceLineNo">207</span>                                        J[2 * i + 1][3] = x / w;<a name="line.207"></a>
<span class="sourceLineNo">208</span>                                        J[2 * i + 1][4] = y / w;<a name="line.208"></a>
<span class="sourceLineNo">209</span>                                        J[2 * i + 1][5] = 1.0 / w;<a name="line.209"></a>
<span class="sourceLineNo">210</span>                                        J[2 * i + 1][6] = -sy * x / w2;<a name="line.210"></a>
<span class="sourceLineNo">211</span>                                        J[2 * i + 1][7] = -sy * y / w2;<a name="line.211"></a>
<span class="sourceLineNo">212</span>                                        J[2 * i + 1][8] = -sy / w2;<a name="line.212"></a>
<span class="sourceLineNo">213</span>                                }<a name="line.213"></a>
<span class="sourceLineNo">214</span>                                return J;<a name="line.214"></a>
<span class="sourceLineNo">215</span>                        }<a name="line.215"></a>
<span class="sourceLineNo">216</span>                };<a name="line.216"></a>
<span class="sourceLineNo">217</span>        }<a name="line.217"></a>
<span class="sourceLineNo">218</span>*/<a name="line.218"></a>
<span class="sourceLineNo">219</span>        <a name="line.219"></a>
<span class="sourceLineNo">220</span>/*<a name="line.220"></a>
<span class="sourceLineNo">221</span>        protected MultivariateMatrixFunction getJacobianFunction(final Point2D[] X) {<a name="line.221"></a>
<span class="sourceLineNo">222</span>                return new MultivariateMatrixFunction() {<a name="line.222"></a>
<span class="sourceLineNo">223</span>                        // See Multi-View Geometry in Computer Vision, eq 4.21, p129<a name="line.223"></a>
<span class="sourceLineNo">224</span>                        @Override<a name="line.224"></a>
<span class="sourceLineNo">225</span>                        public double[][] value(double[] h) {<a name="line.225"></a>
<span class="sourceLineNo">226</span>                                final double[][] J = new double[2 * X.length][9];<a name="line.226"></a>
<span class="sourceLineNo">227</span>                                for (int i = 0; i &lt; X.length; i++) {<a name="line.227"></a>
<span class="sourceLineNo">228</span>                                        final double x = X[i].getX();<a name="line.228"></a>
<span class="sourceLineNo">229</span>                                        final double y = X[i].getY();<a name="line.229"></a>
<span class="sourceLineNo">230</span><a name="line.230"></a>
<span class="sourceLineNo">231</span>                                        final double t2 = x * h[6];<a name="line.231"></a>
<span class="sourceLineNo">232</span>                                        final double t3 = y * h[7];<a name="line.232"></a>
<span class="sourceLineNo">233</span>                                        final double t4 = h[8] + t2 + t3;<a name="line.233"></a>
<span class="sourceLineNo">234</span>                                        final double t5 = 1.0 / t4;<a name="line.234"></a>
<span class="sourceLineNo">235</span>                                        final double t6 = x * h[0];<a name="line.235"></a>
<span class="sourceLineNo">236</span>                                        final double t7 = y * h[1];<a name="line.236"></a>
<span class="sourceLineNo">237</span>                                        final double t8 = h[2] + t6 + t7;<a name="line.237"></a>
<span class="sourceLineNo">238</span>                                        final double t9 = 1.0 / (t4 * t4);<a name="line.238"></a>
<span class="sourceLineNo">239</span>                                        final double t10 = x * t5;<a name="line.239"></a>
<span class="sourceLineNo">240</span>                                        final double t11 = y * t5;<a name="line.240"></a>
<span class="sourceLineNo">241</span>                                        final double t12 = x * h[3];<a name="line.241"></a>
<span class="sourceLineNo">242</span>                                        final double t13 = y * h[4];<a name="line.242"></a>
<span class="sourceLineNo">243</span>                                        final double t14 = h[5] + t12 + t13;<a name="line.243"></a>
<span class="sourceLineNo">244</span>                                        <a name="line.244"></a>
<span class="sourceLineNo">245</span>                                        J[2 * i + 0][0] = t10;<a name="line.245"></a>
<span class="sourceLineNo">246</span>                                        J[2 * i + 0][1] = t11;<a name="line.246"></a>
<span class="sourceLineNo">247</span>                                        J[2 * i + 0][2] = t5;<a name="line.247"></a>
<span class="sourceLineNo">248</span>//                                      J[2 * i + 0][3] = 0;<a name="line.248"></a>
<span class="sourceLineNo">249</span>//                                      J[2 * i + 0][4] = 0;<a name="line.249"></a>
<span class="sourceLineNo">250</span>//                                      J[2 * i + 0][5] = 0;<a name="line.250"></a>
<span class="sourceLineNo">251</span>                                        J[2 * i + 0][6] = -x * t8 * t9;<a name="line.251"></a>
<span class="sourceLineNo">252</span>                                        J[2 * i + 0][7] = -y * t8 * t9;<a name="line.252"></a>
<span class="sourceLineNo">253</span>                                        J[2 * i + 0][8] = -t8 * t9;<a name="line.253"></a>
<span class="sourceLineNo">254</span>                                        <a name="line.254"></a>
<span class="sourceLineNo">255</span>//                                      J[2 * i + 1][0] = 0;<a name="line.255"></a>
<span class="sourceLineNo">256</span>//                                      J[2 * i + 1][1] = 0;<a name="line.256"></a>
<span class="sourceLineNo">257</span>//                                      J[2 * i + 1][2] = 0;<a name="line.257"></a>
<span class="sourceLineNo">258</span>                                        J[2 * i + 1][3] = t10;<a name="line.258"></a>
<span class="sourceLineNo">259</span>                                        J[2 * i + 1][4] = t11;<a name="line.259"></a>
<span class="sourceLineNo">260</span>                                        J[2 * i + 1][5] = t5;<a name="line.260"></a>
<span class="sourceLineNo">261</span>                                        J[2 * i + 1][6] = -x * t9 * t14;<a name="line.261"></a>
<span class="sourceLineNo">262</span>                                        J[2 * i + 1][7] = -y * t9 * t14;<a name="line.262"></a>
<span class="sourceLineNo">263</span>                                        J[2 * i + 1][8] = -t9 * t14;<a name="line.263"></a>
<span class="sourceLineNo">264</span>                                }<a name="line.264"></a>
<span class="sourceLineNo">265</span>                                return J;<a name="line.265"></a>
<span class="sourceLineNo">266</span>                        }<a name="line.266"></a>
<span class="sourceLineNo">267</span>                };<a name="line.267"></a>
<span class="sourceLineNo">268</span>        }<a name="line.268"></a>
<span class="sourceLineNo">269</span> */<a name="line.269"></a>
<span class="sourceLineNo">270</span>        <a name="line.270"></a>
<span class="sourceLineNo">271</span>        <a name="line.271"></a>
<span class="sourceLineNo">272</span>        private double[] transform(double[] p, RealMatrix M3x3) {<a name="line.272"></a>
<span class="sourceLineNo">273</span>                double[] pA = MathUtil.toHomogeneous(p);<a name="line.273"></a>
<span class="sourceLineNo">274</span>                double[] pAt = M3x3.operate(pA);<a name="line.274"></a>
<span class="sourceLineNo">275</span>                return MathUtil.toCartesian(pAt); // need to de-homogenize, since pAt[2] == 1?<a name="line.275"></a>
<span class="sourceLineNo">276</span>        }<a name="line.276"></a>
<span class="sourceLineNo">277</span>        <a name="line.277"></a>
<span class="sourceLineNo">278</span>        private RealMatrix getNormalisationMatrix(Point2D[] pnts) {<a name="line.278"></a>
<span class="sourceLineNo">279</span>                final int N = pnts.length;<a name="line.279"></a>
<span class="sourceLineNo">280</span>                double[] x = new double[N];<a name="line.280"></a>
<span class="sourceLineNo">281</span>                double[] y = new double[N];<a name="line.281"></a>
<span class="sourceLineNo">282</span>                <a name="line.282"></a>
<span class="sourceLineNo">283</span>                for (int i = 0; i &lt; N; i++) {<a name="line.283"></a>
<span class="sourceLineNo">284</span>                        x[i] = pnts[i].getX();<a name="line.284"></a>
<span class="sourceLineNo">285</span>                        y[i] = pnts[i].getY();<a name="line.285"></a>
<span class="sourceLineNo">286</span>                }<a name="line.286"></a>
<span class="sourceLineNo">287</span>                <a name="line.287"></a>
<span class="sourceLineNo">288</span>                // calculate the means in x/y<a name="line.288"></a>
<span class="sourceLineNo">289</span>                double meanx = MathUtil.mean(x);<a name="line.289"></a>
<span class="sourceLineNo">290</span>                double meany = MathUtil.mean(y);<a name="line.290"></a>
<span class="sourceLineNo">291</span><a name="line.291"></a>
<span class="sourceLineNo">292</span>                // calculate the variances in x/y<a name="line.292"></a>
<span class="sourceLineNo">293</span>                double varx = MathUtil.variance(x);<a name="line.293"></a>
<span class="sourceLineNo">294</span>                double vary = MathUtil.variance(y);<a name="line.294"></a>
<span class="sourceLineNo">295</span>                <a name="line.295"></a>
<span class="sourceLineNo">296</span>                double sx = Math.sqrt(2 / varx);<a name="line.296"></a>
<span class="sourceLineNo">297</span>                double sy = Math.sqrt(2 / vary);<a name="line.297"></a>
<span class="sourceLineNo">298</span><a name="line.298"></a>
<span class="sourceLineNo">299</span>                RealMatrix matrixA = MatrixUtils.createRealMatrix(new double[][] {<a name="line.299"></a>
<span class="sourceLineNo">300</span>                                { sx,  0, -sx * meanx},<a name="line.300"></a>
<span class="sourceLineNo">301</span>                                {  0, sy, -sy * meany},<a name="line.301"></a>
<span class="sourceLineNo">302</span>                                {  0,  0,           1 }});<a name="line.302"></a>
<span class="sourceLineNo">303</span>                <a name="line.303"></a>
<span class="sourceLineNo">304</span>                return matrixA;<a name="line.304"></a>
<span class="sourceLineNo">305</span>        }<a name="line.305"></a>
<span class="sourceLineNo">306</span>        <a name="line.306"></a>
<span class="sourceLineNo">307</span><a name="line.307"></a>
<span class="sourceLineNo">308</span>        // TESTING --------------------------------------------------------<a name="line.308"></a>
<span class="sourceLineNo">309</span>        <a name="line.309"></a>
<span class="sourceLineNo">310</span>        static Random rand = new Random();<a name="line.310"></a>
<span class="sourceLineNo">311</span>        private static Point2D apply(RealMatrix H, Point2D X, double noise) {<a name="line.311"></a>
<span class="sourceLineNo">312</span>                <a name="line.312"></a>
<span class="sourceLineNo">313</span>                double[] Xa = {X.getX(), X.getY(), 1};<a name="line.313"></a>
<span class="sourceLineNo">314</span>                double[] Xb = H.operate(Xa);<a name="line.314"></a>
<span class="sourceLineNo">315</span>                double xn = noise * rand.nextGaussian();<a name="line.315"></a>
<span class="sourceLineNo">316</span>                double yn = noise * rand.nextGaussian();<a name="line.316"></a>
<span class="sourceLineNo">317</span>                return new Point2D.Double(xn + Xb[0]/Xb[2], yn + Xb[1]/Xb[2]);<a name="line.317"></a>
<span class="sourceLineNo">318</span>        }<a name="line.318"></a>
<span class="sourceLineNo">319</span>        <a name="line.319"></a>
<span class="sourceLineNo">320</span>        /**<a name="line.320"></a>
<span class="sourceLineNo">321</span>         * Used for testing only.<a name="line.321"></a>
<span class="sourceLineNo">322</span>         * @param args ignored<a name="line.322"></a>
<span class="sourceLineNo">323</span>         */<a name="line.323"></a>
<span class="sourceLineNo">324</span>        public static void main(String[] args) {<a name="line.324"></a>
<span class="sourceLineNo">325</span>                RealMatrix Hreal = MatrixUtils.createRealMatrix(new double[][]<a name="line.325"></a>
<span class="sourceLineNo">326</span>                                {{3, 2, -1},<a name="line.326"></a>
<span class="sourceLineNo">327</span>                                {5, 0, 2},<a name="line.327"></a>
<span class="sourceLineNo">328</span>                                {4, 4, 9}});<a name="line.328"></a>
<span class="sourceLineNo">329</span>                <a name="line.329"></a>
<span class="sourceLineNo">330</span>                List&lt;Point2D&gt; pntlistA = new ArrayList&lt;Point2D&gt;();<a name="line.330"></a>
<span class="sourceLineNo">331</span>                pntlistA.add(new Point2D.Double(10, 7));<a name="line.331"></a>
<span class="sourceLineNo">332</span>                pntlistA.add(new Point2D.Double(3, -1));<a name="line.332"></a>
<span class="sourceLineNo">333</span>                pntlistA.add(new Point2D.Double(5, 5));<a name="line.333"></a>
<span class="sourceLineNo">334</span>                pntlistA.add(new Point2D.Double(-6, 13));<a name="line.334"></a>
<span class="sourceLineNo">335</span>                pntlistA.add(new Point2D.Double(0, 1));<a name="line.335"></a>
<span class="sourceLineNo">336</span>                pntlistA.add(new Point2D.Double(2, 3));<a name="line.336"></a>
<span class="sourceLineNo">337</span>                <a name="line.337"></a>
<span class="sourceLineNo">338</span>                List&lt;Point2D&gt; pntlistB = new ArrayList&lt;Point2D&gt;();<a name="line.338"></a>
<span class="sourceLineNo">339</span>                for (Point2D a : pntlistA) {<a name="line.339"></a>
<span class="sourceLineNo">340</span>                        pntlistB.add(apply(Hreal, a, 0.1));<a name="line.340"></a>
<span class="sourceLineNo">341</span>                }<a name="line.341"></a>
<span class="sourceLineNo">342</span>                <a name="line.342"></a>
<span class="sourceLineNo">343</span>                Point2D[] pntsA = pntlistA.toArray(new Point2D[0]);<a name="line.343"></a>
<span class="sourceLineNo">344</span>                Point2D[] pntsB = pntlistB.toArray(new Point2D[0]);<a name="line.344"></a>
<span class="sourceLineNo">345</span>                <a name="line.345"></a>
<span class="sourceLineNo">346</span>                for (int i = 0; i &lt; pntsA.length; i++) {<a name="line.346"></a>
<span class="sourceLineNo">347</span>                        Point2D a = pntsA[i];<a name="line.347"></a>
<span class="sourceLineNo">348</span>                        Point2D b = pntsB[i];<a name="line.348"></a>
<span class="sourceLineNo">349</span>                        System.out.format("(%.3f, %.3f) -&gt; (%.3f, %.3f)\n", a.getX(), a.getY(), b.getX(), b.getY());<a name="line.349"></a>
<span class="sourceLineNo">350</span>                }<a name="line.350"></a>
<span class="sourceLineNo">351</span>                System.out.println();<a name="line.351"></a>
<span class="sourceLineNo">352</span>                <a name="line.352"></a>
<span class="sourceLineNo">353</span>                HomographyEstimator hest = new HomographyEstimator();<a name="line.353"></a>
<span class="sourceLineNo">354</span>                RealMatrix H = hest.estimateHomography(pntsA, pntsB);<a name="line.354"></a>
<span class="sourceLineNo">355</span>                <a name="line.355"></a>
<span class="sourceLineNo">356</span>                System.out.println("H = "); <a name="line.356"></a>
<span class="sourceLineNo">357</span>                System.out.println(Matrix.toString(H.getData()));<a name="line.357"></a>
<span class="sourceLineNo">358</span>                <a name="line.358"></a>
<span class="sourceLineNo">359</span>                for (Point2D a : pntlistA) {<a name="line.359"></a>
<span class="sourceLineNo">360</span>                        Point2D b = apply(H, a, 0);<a name="line.360"></a>
<span class="sourceLineNo">361</span>                        System.out.format("(%.3f, %.3f) -&gt; (%.3f, %.3f)\n", a.getX(), a.getY(), b.getX(), b.getY());<a name="line.361"></a>
<span class="sourceLineNo">362</span>                }<a name="line.362"></a>
<span class="sourceLineNo">363</span>                <a name="line.363"></a>
<span class="sourceLineNo">364</span>        }<a name="line.364"></a>
<span class="sourceLineNo">365</span><a name="line.365"></a>
<span class="sourceLineNo">366</span>}<a name="line.366"></a>




























































</pre>
</div>
</body>
</html>
